//
// Created by xujingyi on 2021/5/21.
//

#include "solve/SmoothKF.h"
#include "options.h"
namespace rm{
SmoothKF::SmoothKF(const int mode,int state_num,int measure_num,const float &proportion) {
    _mode=mode;
    if(_mode==0){
    _smooth_proportion=proportion;
    _state_num=state_num;
    _measure_num=measure_num;
    Kf = new cv::KalmanFilter(_state_num, _measure_num, 0);
    init();
    return;
    }
    else if(_mode==1){
        _state_num=3;
        _measure_num=3;
        Kf = new cv::KalmanFilter(_state_num, _measure_num, 0);
        init();
        return;
    }
    else if(mode==2){
        _state_num=2;
        _measure_num=2;
        Kf = new cv::KalmanFilter(_state_num, _measure_num, 0);
        init();
        return;
    }
}
#if SHOOTER_MODE==UP_SHOOTER
    void SmoothKF::init() {
        if(_mode==0) {
            Kf->init(_state_num, _measure_num, 0, CV_32FC1);
            //Mat processNoise(stateNum, 1, CV_32F);
            //Mat_<float> measurement_ = Mat::zeros(measure_num, 1, CV_32F);

            Kf->transitionMatrix = cv::Mat::eye(cv::Size(_state_num, _state_num), CV_32FC1);//A 状态转移矩阵
            //这里没有设置控制矩阵B，默认为零
            Kf->measurementMatrix = cv::Mat::eye(_measure_num, _state_num, CV_32FC1);//H=[1,0,0,0;0,1,0,0] 测量矩阵
            setIdentity(Kf->controlMatrix, cv::Scalar::all(0));//H=[1,0,0,0;0,1,0,0] 测量矩阵
            setIdentity(Kf->processNoiseCov, cv::Scalar::all(0.1));//Q高斯白噪声，单位阵
            setIdentity(Kf->measurementNoiseCov, cv::Scalar::all(0.1 * _smooth_proportion));//R高斯白噪声，单位阵
            setIdentity(Kf->errorCovPost, cv::Scalar::all(1));//P后验误差估计协方差矩阵，初始化为单位阵
            randn(Kf->statePost, cv::Scalar::all(0), cv::Scalar::all(0.1));//初始化状态为随机值
            Kf->predict();
            return ;
        }
        else if(_mode==1){//x,y,z
            int d = 3;
            int m = 3;
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->transitionMatrix = (cv::Mat_<float>(d, d) <<
                                                          1.0, 0.0, 0.0,
                    0.0, 1.0, 0.0,
                    0.0, 0.0, 1.0);//A
            Kf->controlMatrix=cv::Scalar::all(0);//B
            Kf->measurementMatrix = cv::Mat::eye(m, d, CV_32FC1);//H
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 10.0f;
            Kf->measurementNoiseCov = cv::Mat::eye(m, m, CV_32FC1);
            Kf->measurementNoiseCov.at<float>(0, 0) = 1000.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 1000.0f;
            Kf->measurementNoiseCov.at<float>(2, 2) =1000.0f;
            Kf->errorCovPost = cv::Mat::eye(d, d, CV_32FC1) * 0.1f;
            return ;
        }
        else if(_mode==2){//
            int d = 2;
            int m = 2;
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->transitionMatrix = (cv::Mat_<float>(d, d) <<
                                                          1.0, 0.0,
                    0.0, 1.0);//A
            Kf->controlMatrix=cv::Scalar::all(0);//B
            Kf->measurementMatrix = cv::Mat::eye(m, d, CV_32FC1);//H
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 10.0f;
            Kf->measurementNoiseCov = cv::Mat::eye(m, m, CV_32FC1);
            Kf->measurementNoiseCov.at<float>(0, 0) = 100.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 100.0f;
            Kf->errorCovPost = cv::Mat::eye(d, d, CV_32FC1) * 0.1f;
            return ;
        }
    };
#elif SHOOTER_MODE==DOWN_SHOOTER
    void SmoothKF::init() {
        if(_mode==0) {
            Kf->init(_state_num, _measure_num, 0, CV_32FC1);
            //Mat processNoise(stateNum, 1, CV_32F);
            //Mat_<float> measurement_ = Mat::zeros(measure_num, 1, CV_32F);

            Kf->transitionMatrix = cv::Mat::eye(cv::Size(_state_num, _state_num), CV_32FC1);//A 状态转移矩阵
            //这里没有设置控制矩阵B，默认为零
            Kf->measurementMatrix = cv::Mat::eye(_measure_num, _state_num, CV_32FC1);//H=[1,0,0,0;0,1,0,0] 测量矩阵
            setIdentity(Kf->controlMatrix, cv::Scalar::all(0));//H=[1,0,0,0;0,1,0,0] 测量矩阵
            setIdentity(Kf->processNoiseCov, cv::Scalar::all(0.1));//Q高斯白噪声，单位阵
            setIdentity(Kf->measurementNoiseCov, cv::Scalar::all(0.1 * _smooth_proportion));//R高斯白噪声，单位阵
            setIdentity(Kf->errorCovPost, cv::Scalar::all(1));//P后验误差估计协方差矩阵，初始化为单位阵
            randn(Kf->statePost, cv::Scalar::all(0), cv::Scalar::all(0.1));//初始化状态为随机值
            Kf->predict();
            return ;
        }
        else if(_mode==1){//x,y,z
            int d = 3;
            int m = 3;
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->transitionMatrix = (cv::Mat_<float>(d, d) <<
                                                          1.0, 0.0, 0.0,
                    0.0, 1.0, 0.0,
                    0.0, 0.0, 1.0);//A
            Kf->controlMatrix=cv::Scalar::all(0);//B
            Kf->measurementMatrix = cv::Mat::eye(m, d, CV_32FC1);//H
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 10.0f;
            Kf->measurementNoiseCov = cv::Mat::eye(m, m, CV_32FC1);
            Kf->measurementNoiseCov.at<float>(0, 0) = 1000.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 1000.0f;
            Kf->measurementNoiseCov.at<float>(2, 2) =1000.0f;
            Kf->errorCovPost = cv::Mat::eye(d, d, CV_32FC1) * 0.1f;
            return ;
        }
        else if(_mode==2){//
            int d = 2;
            int m = 2;
            Kf->statePre=cv::Scalar::all(0);
            Kf->statePost==cv::Scalar::all(0);
            Kf->transitionMatrix = (cv::Mat_<float>(d, d) <<
                                                          1.0, 0.0,
                    0.0, 1.0);//A
            Kf->controlMatrix=cv::Scalar::all(0);//B
            Kf->measurementMatrix = cv::Mat::eye(m, d, CV_32FC1);//H
            Kf->processNoiseCov = cv::Mat::eye(d, d, CV_32FC1) * 10.0f;
            Kf->measurementNoiseCov = cv::Mat::eye(m, m, CV_32FC1);
            Kf->measurementNoiseCov.at<float>(0, 0) = 100.0f;
            Kf->measurementNoiseCov.at<float>(1, 1) = 100.0f;
            Kf->errorCovPost = cv::Mat::eye(d, d, CV_32FC1) * 0.1f;
            return ;
        }
    };
#endif

float SmoothKF::renew(const float &num){
    _measurement1.at<float>(0) = (float)num; //当前yaw
    Kf->correct(_measurement1);//
    cv::Mat prediction =  Kf->predict(); //预测并返回下一时刻状态值
    return prediction.at<float>(0);
};

cv::Point2f SmoothKF::renew(const cv::Point2f &point2f){
    _measurement2.at<float>(0) = point2f.x; //当前yaw
    _measurement2.at<float>(1) = point2f.y; //当前pitch
    Kf->correct(_measurement2);//
    cv::Mat prediction =  Kf->predict(); //预测并返回下一时刻状态值
    return cv::Point2f(prediction.at<float>(0), prediction.at<float>(1));
}

cv::Point3f SmoothKF::renew(const cv::Point3f &point3f){
    _measurement3.at<float>(0) = point3f.x; //当前yaw
    _measurement3.at<float>(1) = point3f.y; //当前pitch
    _measurement3.at<float>(2) = point3f.z; //当前z
    Kf->correct(_measurement3);//
    cv::Mat prediction =  Kf->predict(); //预测并返回下一时刻状态值
    return cv::Point3f(prediction.at<float>(0), prediction.at<float>(1), prediction.at<float>(2));
};

};